﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems
{
    public class System_DoubleInvertedPendulum : System_SimplifiedPlanarPhysicalRobot
    {
        double m0 = 0.1;
        double m1 = 1;
        double m2 = 1;
        double L1 = 1.5;
        double L2 = 1.5;
        double g = 9.80665;

        override public bool GoalReached(double[] q_test, double[] q_target, double tol)
        {
            double theta1 = q_test[0];
            double theta2 = q_test[1];
            double theta3 = q_test[2];
            double dottheta1 = q_test[3];
            double dottheta2 = q_test[4];
            double dottheta3 = q_test[5];

            double length = Math.Sqrt(
            theta2 * theta2 +
            theta3 * theta3 +
            dottheta1 * dottheta1 +
            dottheta2 * dottheta2 +
            dottheta3 * dottheta3);

            if (length < 0.1)
                return true;
            else
                return false;
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            double[] D = new double[9];
            double[] C = new double[9];
            double[] G = new double[3];
            double[] H = new double[3];
            double[] D_inv = new double[9];
            double[] DiC = new double[9];

            double theta1 = q[0];
            double theta2 = q[1];
            double theta3 = q[2];
            double dottheta1 = q[3];
            double dottheta2 = q[4];
            double dottheta3 = q[5];
            double u = ctrl[0];

            double d1 = m0 + m1 + m2;
            double d2 = (0.5 * m1 + m2) * L1;
            double d3 = 0.5 * m2 * L2;
            double d4 = (m1 / 3 + m2) * L1 * L1;
            double d5 = 0.5 * m2 * L1 * L2;
            double d6 = m2 / 3 * L2 * L2;
            double f1 = (0.5 * m1 + m2) * L1 * g;
            double f2 = 0.5 * m2 * L2 * g;

            D[0] = d1;
            D[1] = d2 * Math.Cos(theta1);
            D[2] = d3 * Math.Cos(theta2);
            D[3] = D[1];
            D[4] = d4;
            D[5] = d5 * Math.Cos(theta1 - theta2);
            D[6] = D[2];
            D[7] = D[5];
            D[8] = d6;
            C[0] = 0;
            C[1] = -d2 * Math.Sin(theta1) * dottheta1;
            C[2] = -d3 * Math.Sin(theta2) * dottheta2;
            C[3] = 0;
            C[4] = 0;
            C[5] = d5 * Math.Sin(theta1 - theta2) * dottheta2;
            C[6] = 0;
            C[7] = -d5 * Math.Sin(theta1 - theta2) * dottheta1;
            C[8] = 0;
            G[0] = 0;
            G[1] = -f1 * Math.Sin(theta1);
            G[2] = -f2 * Math.Sin(theta2);
            H[0] = 1;
            H[1] = 0;
            H[2] = 0;

            MotionPlanningUtils.Matrix_Inverse_3x3(D, ref D_inv);
            MotionPlanningUtils.Matrix_Multiply_3x3(D_inv, C, ref DiC);
            
            qd[0] = dottheta1;
            qd[1] = dottheta2;
            qd[2] = dottheta3;
            qd[3] = -DiC[0] * dottheta1 - DiC[1] * dottheta2 - DiC[2] * dottheta3 - D_inv[0] * G[0] - D_inv[1] * G[1] - D_inv[2] * G[2] + D_inv[0] * u;
            qd[4] = -DiC[3] * dottheta1 - DiC[4] * dottheta2 - DiC[5] * dottheta3 - D_inv[3] * G[0] - D_inv[4] * G[1] - D_inv[5] * G[2] + D_inv[3] * u;
            qd[5] = -DiC[6] * dottheta1 - DiC[7] * dottheta2 - DiC[8] * dottheta3 - D_inv[6] * G[0] - D_inv[7] * G[1] - D_inv[8] * G[2] + D_inv[6] * u;
        }

        override public void Integration(RK4Helper pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {
            base.Integration(pad, q, ctrl, t_delta, ref q_next);
            double PI_double = Math.PI + Math.PI;
            while (q_next[1] > Math.PI)
                q_next[1] -= PI_double;
            while (q_next[1] < -Math.PI)
                q_next[1] += PI_double;
            while (q_next[2] > Math.PI)
                q_next[2] -= PI_double;
            while (q_next[2] < -Math.PI)
                q_next[2] += PI_double;
        }
    }
}
